#include "powerDrive.hpp"
#include "../Global/GlobalDefs.hpp"
#include "../Global/globalVars.hpp"
#include "../Interface/report.hpp"
#include "tempUpdate.hpp"
#include <arduino.h>

// #define PWM0_Ch 0
#define PWM0_Res 7
#define PWM0_DUTY_NORMALIZER_K 127.0f / 100.0f
// #define PWM0_DUTY_NORMALIZER_B -127
#define PWM0_DUTY_NORMALIZER_B 0
#define PWM0_Freq 312500/2 // 40000000/2^PWM0_Res
//#define PWM0_Freq 78125 // 40000000/2^PWM0_Res
// #define PWM0_Freq 40

// cannot use 1: this cause both channel to be one the same frequency
// #define PWM1_Ch 0
#define PWM1_Res 7
// #define PWM1_DUTY_NORMALIZER_K 128.0f/100.0f
// #define PWM1_DUTY_NORMALIZER_B -100
#define PWM1_Freq  312500/2 // 40000000/2^PWM1_Res
//#define PWM1_Freq 78125 // 40000000/2^PWM1_Res
// #define PWM1_Freq 90

#define MAIN_PWM_Res PWM0_Res
#define MAIN_PWM_Freq PWM0_Freq

// cannot use 1: this cause both channel to be one the same frequency
// #define PWM1_Ch 0
#define PWM2_Res 7
// #define PWM1_DUTY_NORMALIZER_K 128.0f/100.0f
// #define PWM1_DUTY_NORMALIZER_B -100
//#define PWM1_Freq  312500 // 40000000/2^PWM1_Res
#define PWM2_Freq 3125 // 40000000/2^PWM1_Res
// #define PWM1_Freq 90

#define FAN_PWM_Res PWM2_Res
#define FAN_PWM_Freq PWM2_Freq

#define DRIVE_PIN_0_PWM_CHNL 0
#define DRIVE_PIN_1_PWM_CHNL 1
#define DRIVE_PIN_2_PWM_CHNL 2
// channel 3/4 has bug, at least on one board. 
// Just reserve 3~7 for other single-channel controllers
#define DRIVE_PIN_3_PWM_CHNL 8
#define DRIVE_PIN_4_PWM_CHNL 9
#define DRIVE_PIN_5_PWM_CHNL 10
#define DRIVE_PIN_6_PWM_CHNL 11
// XXX not anymore pwm=100% sets drive off
// XXX not anymore pwm=0% set drive max
// #define OFF_DUTY 127
// #define FULL_DUTY 0
int MIN_DUTY = 0;
int MAX_DUTY = 127;
#define OFF_LV LOW
#define OFF_DUTY MIN_DUTY
#define FULL_DUTY MAX_DUTY
#define NUM_CHANNEL 3
#define NUM_SUBCHANNEL 3

// Referenced by [Channel][SubChannel]
int virtual2realChannelMap[NUM_CHANNEL][NUM_SUBCHANNEL] = { DRIVE_PIN_0_PWM_CHNL, DRIVE_PIN_3_PWM_CHNL, DRIVE_PIN_5_PWM_CHNL \
														  , DRIVE_PIN_1_PWM_CHNL, DRIVE_PIN_4_PWM_CHNL, DRIVE_PIN_6_PWM_CHNL \
														  , DRIVE_PIN_2_PWM_CHNL, 0xff			      , 0xff				 \
};
int bitNums[3] = { PWM0_Res, PWM1_Res, PWM2_Res };

int outputPercentForSimu[NUM_CHANNEL][NUM_SUBCHANNEL] = { 0 };

bool PowerDrive::flagIsEnabled = false;

void initCh_core(Channel ch, SubChannel subCh, uint8_t pwmCh, uint8_t physicalPin, uint32_t freq, uint8_t res) {
	ledcSetup(pwmCh, freq, res);
	ledcWrite(pwmCh, OFF_DUTY);
	ledcAttachPin(physicalPin, pwmCh);
	Serial.print("Ch ");
	Serial.print(ch);
	Serial.print("-");
	Serial.print(subCh);
	Serial.print(", pin ");
	Serial.print(physicalPin);
	Serial.print(", mapped to ");
	Serial.println(pwmCh);
}

bool PowerDrive::init()
{
#ifdef MODEL_RES_HEATER
	pinMode(PRIM_DR_PIN, OUTPUT);
	digitalWrite(PRIM_DR_PIN, OFF_LV);
	pinMode(SEC_DR_PIN, OUTPUT);
	digitalWrite(SEC_DR_PIN, OFF_LV);
	pinMode(THIRD_DR_PIN, OUTPUT);
	digitalWrite(THIRD_DR_PIN, OFF_LV);

	initCh_core(Channel::Heat, SubChannel::All, virtual2realChannelMap[Channel::Heat][SubChannel::All], PRIM_DR_PIN, MAIN_PWM_Freq, MAIN_PWM_Res);
	initCh_core(Channel::Cool, SubChannel::All, virtual2realChannelMap[Channel::Cool][SubChannel::All], SEC_DR_PIN, MAIN_PWM_Freq, MAIN_PWM_Res);
	initCh_core(Channel::Fan, SubChannel::All, virtual2realChannelMap[Channel::Fan][SubChannel::All], THIRD_DR_PIN, FAN_PWM_Freq, FAN_PWM_Res);
#endif
#ifdef MODEL_TEC_CYCLER

	pinMode(MULTI_PRIM_DR_PIN0, OUTPUT);
	digitalWrite(MULTI_PRIM_DR_PIN0, OFF_LV);
	pinMode(MULTI_PRIM_DR_PIN1, OUTPUT);
	digitalWrite(MULTI_PRIM_DR_PIN1, OFF_LV);
	pinMode(MULTI_PRIM_DR_PIN2, OUTPUT);
	digitalWrite(MULTI_PRIM_DR_PIN2, OFF_LV);
	pinMode(MULTI_SEC_DR_PIN0, OUTPUT);
	digitalWrite(MULTI_SEC_DR_PIN0, OFF_LV);
	pinMode(MULTI_SEC_DR_PIN1, OUTPUT);
	digitalWrite(MULTI_SEC_DR_PIN1, OFF_LV);
	pinMode(MULTI_SEC_DR_PIN2, OUTPUT);
	digitalWrite(MULTI_SEC_DR_PIN2, OFF_LV);
	pinMode(FAN_DR_PIN, OUTPUT);
	digitalWrite(FAN_DR_PIN, OFF_LV);

	initCh_core(Channel::Heat, SubChannel::Left, virtual2realChannelMap[Channel::Heat][SubChannel::Left], MULTI_PRIM_DR_PIN0, MAIN_PWM_Freq, MAIN_PWM_Res);
	initCh_core(Channel::Heat, SubChannel::Center, virtual2realChannelMap[Channel::Heat][SubChannel::Center], MULTI_PRIM_DR_PIN1, MAIN_PWM_Freq, MAIN_PWM_Res);
	initCh_core(Channel::Heat, SubChannel::Right, virtual2realChannelMap[Channel::Heat][SubChannel::Right], MULTI_PRIM_DR_PIN2, MAIN_PWM_Freq, MAIN_PWM_Res);

	initCh_core(Channel::Cool, SubChannel::Left, virtual2realChannelMap[Channel::Cool][SubChannel::Left], MULTI_SEC_DR_PIN0, MAIN_PWM_Freq, MAIN_PWM_Res);
	initCh_core(Channel::Cool, SubChannel::Center, virtual2realChannelMap[Channel::Cool][SubChannel::Center], MULTI_SEC_DR_PIN1, MAIN_PWM_Freq, MAIN_PWM_Res);
	initCh_core(Channel::Cool, SubChannel::Right, virtual2realChannelMap[Channel::Cool][SubChannel::Right], MULTI_SEC_DR_PIN2, MAIN_PWM_Freq, MAIN_PWM_Res);

	initCh_core(Channel::Fan, SubChannel::Left, virtual2realChannelMap[Channel::Fan][SubChannel::Left], FAN_DR_PIN, FAN_PWM_Freq, FAN_PWM_Res);
#endif
	// pinMode(DRIVE_DISABLE, OUTPUT);
	disable();

	return true;
}

void PowerDrive::enable()
{
	flagIsEnabled = true;
	// digitalWrite(DRIVE_DISABLE, LOW);
}

void PowerDrive::disable()
{
	flagIsEnabled = false;
	GlobalVars::heatUpDrivePercent = 0;
	PowerDrive::setChannelPercent(Heat, All, GlobalVars::heatUpDrivePercent);
	GlobalVars::coolDownDrivePercent = 0;
	PowerDrive::setChannelPercent(Cool, All, GlobalVars::coolDownDrivePercent);
	GlobalVars::temptarget = 0;
	GlobalVars::mean_Reading = 0;
	GlobalVars::speed = 0;
	GlobalVars::ctrlPlan.flagStartTempChange = 0;
#ifdef FAN_CONTROL_CHANNEL
	GlobalVars::fanDrivePercent = 0;
	PowerDrive::setChannelPercent(Fan, All, GlobalVars::fanDrivePercent);
#endif
}

bool PowerDrive::isEnabled()
{
	return flagIsEnabled;
}

int percentage2duty(int channel, uint8_t percentage)
{
	if (percentage == 0)
	{
		return OFF_DUTY;
	}
	if (percentage == 100)
	{
		return FULL_DUTY;
	}
	// not right if k<0
	int duty = round(percentage * PWM0_DUTY_NORMALIZER_K + PWM0_DUTY_NORMALIZER_B);
	return max(MIN_DUTY, min(MAX_DUTY, duty));
}

bool PowerDrive::setChannelPercent(Channel channel, SubChannel subChannel, uint8_t percentage)
{
	if (channel == Channel::Heat || channel == Channel::Cool || channel == Channel::Fan) {
		int duty = percentage2duty((int)channel, percentage);
		if (GlobalVars::flagSimuRate == 0)
		{
			if (channel == 2)
			{
				ledcWrite(virtual2realChannelMap[channel][SubChannel::Center], duty);

				outputPercentForSimu[channel][SubChannel::Center] = duty;
				outputPercentForSimu[channel][SubChannel::Left] = duty;
				outputPercentForSimu[channel][SubChannel::Right] = duty;
			}
			else if (subChannel == SubChannel::All) {
				ledcWrite(virtual2realChannelMap[channel][SubChannel::Center], duty);
				ledcWrite(virtual2realChannelMap[channel][SubChannel::Left], duty);
				ledcWrite(virtual2realChannelMap[channel][SubChannel::Right], duty);

				outputPercentForSimu[channel][SubChannel::Center] = duty;
				outputPercentForSimu[channel][SubChannel::Left] = duty;
				outputPercentForSimu[channel][SubChannel::Right] = duty;
			}
			else
			{
				ledcWrite(virtual2realChannelMap[channel][subChannel], duty);
				outputPercentForSimu[channel][subChannel] = duty;

				if (GlobalVars::flagVerboseMode > 0)
				{
					Serial.print("The channel being used is ");
					Serial.println(channel);
					Serial.print("The part of TEC being used is ");
					Serial.println(subChannel);
					Serial.print("The percentage is ");
					Serial.println(percentage);
				}
			}

		}
		return true;
	}
	else
	{
		return false;
	}
}



bool PowerDrive::setChannelPercentDur(int channel, int subchannel, int percentage, int durationSec)
{
	PowerDrive::enable();
	for (int pUp = 0; pUp < percentage; pUp++) {
		setChannelPercent((Channel)channel, (SubChannel)subchannel, (uint8_t)pUp);
		delayMicroseconds(100);
	}
	if (durationSec > 0)
	{
		for (int secIdx = 0; secIdx < durationSec; secIdx++)
		{
			if (Serial.available())
			{
				break;
			}
			Serial.println("Count down {" + String(durationSec - secIdx) + "}...");
			Serial.print("Cen ");
			Serial.print(TempReader::readPlateTmp());
			Serial.print("Pump ");
			Serial.println(TempReader::readPumpTmp());
			Serial.print("Left ");
			Serial.print(TempReader::readSampleLeftTmp());
			Serial.print("Right ");
			Serial.println(TempReader::readSampleRightTmp());
			delay(1000);

		}

		PowerDrive::disable();
		setChannelPercent((Channel)channel, (SubChannel)subchannel, 0);
		Serial.println("Stopped");
		return false;
	}
	else
	{
		Serial.println("No auto stop");
	}
	return true;
}

#define TestPWMReadoutInterval 100
bool setChPercIncDecShared(int time, int total, int perc) {
	Serial.print("Count down ");
	Serial.print(time);
	Serial.print("/");
	Serial.print(total);
	Serial.print(" @");
	Serial.print(perc);
	Serial.println("%");
	for (int subSec = 0; subSec < 1000 / TestPWMReadoutInterval; subSec++) {
		Serial.print("  Cen ");
		Serial.print(TempReader::readPlateTmp());
		Serial.print(", Pump ");
		Serial.print(TempReader::readPumpTmp());
		Serial.print(",  Left ");
		Serial.print(TempReader::readSampleLeftTmp());
		Serial.print(", Right ");
		Serial.println(TempReader::readSampleRightTmp());
		delay(TestPWMReadoutInterval);
		if (Serial.available())
		{
			return true;
		}
	}
	return false;
}

bool PowerDrive::setChannelPercentDecrease(int channel, int subchannel, int percentage, int durationSec)
{
	PowerDrive::enable();
	setChannelPercent((Channel)channel, (SubChannel)subchannel, percentage);
	if (durationSec > 0)
	{
		for (int secIdx = 0; secIdx < durationSec; secIdx++)
		{
			if (Serial.available())
			{
				break;
			}
			int perc = percentage * ((float)durationSec - (float)secIdx) / (float)durationSec;
			setChannelPercent((Channel)channel, (SubChannel)subchannel, perc);
			if (setChPercIncDecShared(secIdx, durationSec, perc)) {
				break;
			}
		}

		PowerDrive::disable();
		Serial.println("Stopped");
		return false;
	}
	else
	{
		Serial.println("No auto stop");
	}
	return true;
}

bool PowerDrive::setChannelPercentIncrease(int channel, int subchannel, int percentage, int durationSec)
{
	PowerDrive::enable();
	if (durationSec > 0)
	{
		for (int secIdx = 0; secIdx < durationSec; secIdx++)
		{
			if (Serial.available())
			{
				break;
			}
			int perc = percentage * (float)(secIdx + 1) / (float)durationSec;
			setChannelPercent((Channel)channel, (SubChannel)subchannel, perc);
			if (setChPercIncDecShared(secIdx, durationSec, perc)) {
				break;
			}
		}

		PowerDrive::disable();
		Serial.println("Stopped");
		return false;
	}
	else
	{
		Serial.println("No auto stop");
	}
	return true;
}

bool PowerDrive::setChannelFreq(int channel, int subchannel, int frequencyHz)
{
	Serial.print("Channel 0 frequency was ");
	Serial.println(ledcReadFreq(virtual2realChannelMap[0][0]));
	Serial.println(ledcReadFreq(virtual2realChannelMap[0][1]));
	Serial.println(ledcReadFreq(virtual2realChannelMap[0][2]));
	Serial.print("Channel 1 frequency was ");
	Serial.println(ledcReadFreq(virtual2realChannelMap[1][0]));
	Serial.println(ledcReadFreq(virtual2realChannelMap[1][1]));
	Serial.println(ledcReadFreq(virtual2realChannelMap[1][2]));
	Serial.print("Channel 2 frequency was ");
	Serial.println(ledcReadFreq(virtual2realChannelMap[2][0]));
	if (channel == 0 || channel == 1 || channel == 2)
	{

		ledcChangeFrequency(virtual2realChannelMap[channel][subchannel], frequencyHz, bitNums[channel]);
		// ledcSetup(DRIVE_PIN_0_PWM_CHNL, frequencyHz, PWM0_Res);
		Serial.print("Channel " + String(channel) + " frequency set to ");
		Serial.println(frequencyHz);
		return true;
	}
	return false;
}

double PowerDrive::getChannelFreq(int channel, int subchannel)
{
	return ledcReadFreq(virtual2realChannelMap[channel][subchannel]);
}

static uint32_t cDuties[9];
void PowerDrive::holdOn()
{
	for (int ch = 0; ch <= 2; ch++) {
		for (int sh = 0; sh <= 2; sh++) {
			cDuties[ch] = ledcRead(virtual2realChannelMap[ch][sh]);
			ledcWrite(virtual2realChannelMap[ch][sh], OFF_DUTY);
		}
	}
}

void PowerDrive::holdOff()
{
	for (int ch = 0; ch <= 2; ch++) {
		for (int sh = 0; sh <= 2; sh++) {
			ledcWrite(virtual2realChannelMap[ch][sh], cDuties[ch]);
		}
	}
}

SubChannel otherSide(SubChannel subChannel) {
	if (subChannel == Left) {
		return Right;
	}
	else {
		return Left;
	}
}

int PowerDrive::getFanPercent() {
	return outputPercentForSimu[Channel::Fan][SubChannel::Center];
}

int PowerDrive::getChannelNetWeightedPercent(SubChannel subChannel) {
	int selfHeat = 9;
	int crossHeatCenterSide = 3;
	int crossHeatSideSide = 1;

	int percWeightedSum = 0, weightSum = 0;
	if (subChannel == SubChannel::Center) {
		percWeightedSum = selfHeat * (outputPercentForSimu[Channel::Heat][SubChannel::Center] - outputPercentForSimu[Channel::Cool][SubChannel::Center]);
		percWeightedSum += crossHeatCenterSide * (outputPercentForSimu[Channel::Heat][SubChannel::Left] - outputPercentForSimu[Channel::Cool][SubChannel::Left]);
		percWeightedSum += crossHeatCenterSide * (outputPercentForSimu[Channel::Heat][SubChannel::Right] - outputPercentForSimu[Channel::Cool][SubChannel::Right]);
		weightSum = selfHeat + crossHeatCenterSide + crossHeatCenterSide;
	}
	else {
		percWeightedSum = selfHeat * (outputPercentForSimu[Channel::Heat][subChannel] - outputPercentForSimu[Channel::Cool][subChannel]);
		percWeightedSum += crossHeatCenterSide * (outputPercentForSimu[Channel::Heat][SubChannel::Center] - outputPercentForSimu[Channel::Cool][SubChannel::Center]);
		percWeightedSum += crossHeatSideSide * (outputPercentForSimu[Channel::Heat][otherSide(subChannel)] - outputPercentForSimu[Channel::Cool][otherSide(subChannel)]);
		weightSum = selfHeat + crossHeatCenterSide + crossHeatSideSide;
	}

	return percWeightedSum / weightSum;
}
